/*============================================================================*/
/*                         The Freescale Cup                                 */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* %name:            %	APPLeds.c
* %version:         %	1.0
* %created_by:      %   Continental
* %date_created:    %   Sun Aug 21 16:20:00 2011 
*=============================================================================*/
/* DESCRIPTION : Contains applications that use the TRK-MPC5604B LEDS         */
/*============================================================================*/
/* FUNCTION COMMENT : comment here										      */
/*                                                                            */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 21/08/2011  |                               | Continental     */
/* Application using LEDS to show the scheduler working                       */
/*============================================================================*/

/* Includes */
/* -------- */
#include "stdtypedef.h"
#include "APPControl.h"
#include "MALADC.h"
#include "MALioapi.h"


#include "math.h"


//Varibles que usaran otras funciones
T_SWORD ruw_WRM;
T_SWORD ruw_WLM;

T_UBYTE rub_Side;
T_UWORD ruw_Volante;

//Varibles que usa de otras funciones 
extern T_SBYTE rsb_CenterLineVel;
extern T_UBYTE rub_BlancoVel;
extern T_SBYTE rsb_CenterLinePos;
extern T_UBYTE rub_BlancoPos;

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 


/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/* Private functions prototypes */
/* ---------------------------- */

/* Exported functions prototypes */
/* ----------------------------- */


/**************************************************************
*  Name                 : Led1_init
*  Description          : Prepare Led1 application.
						  This function initialize Led1_app 
*  Parameters           :  none
*  Return               :  none
*  Critical/explanation :  No
**************************************************************/
void APPContro_PID(void)
{
	
	static T_SWORD rsw_KpDir = 48;
	static T_SWORD rsw_KiDir = 0;
	static T_SWORD rsw_KdDir = 10;//3
	
  	T_SWORD rsw_ukDir; //Output control signal uk
	T_SWORD rsw_ekDir; //Error ek
	
	static T_SWORD rsw_ek0Dir = 0; //Error ek-1
    static T_SWORD rsw_ek1Dir = 0; //Error ek-2
    static T_SWORD rsw_uk0Dir = 0; //Salida yk-1
    
	//Left control varible
	static T_SWORD rsw_aDir; 
	static T_SWORD rsw_bDir;
	static T_SWORD rsw_cDir;
  	
  	static T_UBYTE rub_Side0 = 0;
    static T_UWORD ruw_Volante0 = 0;
  	
  	rsw_aDir = rsw_KpDir + rsw_KiDir + rsw_KdDir;
  	rsw_bDir = (-1) * ( rsw_KpDir + (2*rsw_KdDir) );
  	rsw_cDir = rsw_KdDir;
  	
   	rsw_ekDir = (T_SWORD)(rsb_CenterLineVel); //Calculo del error
  	rsw_ukDir = rsw_uk0Dir + (rsw_aDir * rsw_ekDir) + (rsw_bDir * rsw_ek0Dir) + (rsw_cDir * rsw_ek1Dir) ; //Salida
    
    ////Limites////////////
    if (rsw_ukDir >= 2500)
    {
    	rsw_ukDir = 2500;
    }
    
    if (rsw_ukDir <= -2500)
    {
    	rsw_ukDir = -2500;
    }
    
    
    if(rsw_ukDir >= 0)
    {
    	rub_Side = 0;
    	ruw_Volante = (T_UWORD)(rsw_ukDir);
    }
    else
    {
    	rub_Side = 1;
    	ruw_Volante = (T_UWORD)(rsw_ukDir * (-1));
    }
    
    if (rub_BlancoVel==1)
    {
    	rub_Side = rub_Side0;
    	ruw_Volante = ruw_Volante0;
    }
      	
  	//Guardar Variables
   	rsw_ek1Dir = rsw_ek0Dir;
   	rsw_ek0Dir = rsw_ekDir;
   	rsw_uk0Dir = rsw_ukDir;
    rub_Side0 = rub_Side;
    ruw_Volante0 = ruw_Volante;
   
}
/**************************************************************
*  Name                 : FUNCION DEL PIN  1 para activar el driver de m1
*  Description          : Prepare Led1 application.
						  This function initialize Led1_app 
*  Parameters           :  none
*  Return               :  none
*  Critical/explanation :  No
**************************************************************/
void APPControl_Kinematics(void)
{
	double rdo_Ancho = 140.00;//Ancho del carro en milimetros
	double rdo_Largo = 195.00;//Largo del carro en milimetros
	double rdo_Radio = 20.50; //Radio del la llanta del carrito (milimetros)
	double rdo_VEL = 0.70; //Fijar velocidad lineal del carrito en metros/segundo
	double pi = 3.141592654;

	/////Variables para la cinematica///
 	double rdo_R1; //El radio de giro
 	double rdo_W;
 	double rdo_RIn;
 	double rdo_ROut;
 	double rdo_WIn;
 	double rdo_WOut;
 	double rdo_VelIn;
 	double rdo_VelOut;
	double rdo_VolanteRad;
	
	if(ruw_Volante <= 50) 
   {
   		rdo_VEL = rdo_VEL * 1000;
   		rdo_WOut = rdo_VEL / rdo_Radio;
   		rdo_WIn = rdo_WOut;
   }
   else
   {
   
   		rdo_VolanteRad = (( (ruw_Volante * 1.5) / 100) * pi) / 180 ; //Convercion a radianes el angulo del volante.
   
   		rdo_R1 = rdo_Largo / (tan(rdo_VolanteRad)); //Calculo del radio de giro (milimetros)
   
   		rdo_VEL = rdo_VEL * 1000; //Convercion de velocidad a milimetros/segundo 
   
   		rdo_W = rdo_VEL / rdo_R1; //Velocidad angular respecto al centro de giro (radianes/segundo)
   
   		rdo_RIn = rdo_R1 - (rdo_Ancho/2); //Radio de giro de la llanta de adentro(milimetros)
   
   		rdo_ROut = rdo_R1 + (rdo_Ancho/2); //Radio de giro de la llanta de afuera (milimetros)
   
   		//Calculo de velocidad lineal
   		rdo_VelIn = rdo_W * rdo_RIn; //Velocidad lineal de la llanta de adentro (milimetros/segundo)
   		rdo_VelOut = rdo_W * rdo_ROut; //Velocidad lineal de la llanta de afuera (milimetros/segundo)
	
   		//Calculo de velcidad angular 
   		rdo_WIn = rdo_VelIn / rdo_Radio; //Velocidad angular de la llanta de adentro (radianes/segundo)
   		rdo_WOut = rdo_VelOut / rdo_Radio; //Velocidad angular de la llanta de afuera (radianes/segundo)
   }
   
   
   //Asignacion de velocidades angulares
   if (rub_Side == 0)//Para que gire a la derecha
   {
   		ruw_WRM = (T_UWORD)(rdo_WIn);
   		ruw_WLM = (T_UWORD)(rdo_WOut);
   }
   if (rub_Side == 1) //Para que gire a la izquierda
   {
   		ruw_WRM = (T_UWORD)(rdo_WOut);
   		ruw_WLM = (T_UWORD)(rdo_WIn);
   }
  	
}

/**************************************************************
*  Name                 : Inicializa el ADC (G.Proporcional, G.Derivativa, 
*  Description          : G.Integral, V. en rectas y V. en curvas) 
						   
*  Parameters           :  none
*  Return               :  none
*  Critical/explanation :  No
**************************************************************/
void init_adc_control(void)
{
	ConfigPinAs_ADC_Channel(pin_PD0); //G. PROPORCIONAL
 	ConfigPinAs_ADC_Channel(pin_PD1); //G. DERIVATIVA
 	ConfigPinAs_ADC_Channel(pin_PD2); //G. INTEGRAL
 	ConfigPinAs_ADC_Channel(pin_PD3); //VELOCIDAD EN RECTAS
 	ConfigPinAs_ADC_Channel(pin_PB4); //VELOCIDAD EN CURVAS
 	Config_register_MCR(register_mcr_control);//Camera_Position and Camera_Velocity
 	SetAmountOfChannels( adc_channel_4_to_8,Ninguno_32_47,Ninguno_64_95 ); //Camera_Position and Camera_Velocity
 	SetConvertionTime(ADC_CTR0_Control, ADC_CTR1, ADC_CTR2); //Camera_Porition and Camera_Velocity	
}



	

